/**
 * @file six_axis_sensor_can_sdk.cpp
 * @brief STM32G431六维力传感器CAN总线C++ SDK实现 (简化版)
 * @version 1.0.0
 * @date 2025-01-14
 */

#include "six_axis_sensor_can_sdk.h"
#include <iostream>
#include <thread>
#include <atomic>
#include <mutex>
#include <condition_variable>
#include <sstream>
#include <iomanip>
#include <cstring>
#include <cmath>
#include <fstream>
#include <algorithm>

// Linux CAN相关头文件
#ifdef __linux__
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <linux/can/error.h>
#include <unistd.h>
#include <fcntl.h>
#include <poll.h>
#include <sys/uio.h>
#endif

namespace SixAxisSensorCAN {

/**
 * @brief CAN消息ID定义
 */
// CAN消息ID将根据传感器ID动态计算，不再使用静态常量

/**
 * @brief SDK实现类 (PIMPL idiom)
 */
class SixAxisSensorCANSDK::Impl {
public:
    Impl() : status_(SensorStatus::DISCONNECTED),
             can_socket_(-1),
             debug_mode_(false),
             sensor_id_(0x64),  // 默认传感器ID
             can_listener_running_(false) {
    }
    
    ~Impl() {
        stopCANListenerImpl();
        closeCANImpl();
    }
    
    // 成员变量
    SensorStatus status_;
    int can_socket_;
    std::string interface_name_;
    bool debug_mode_;
    uint32_t sensor_id_;  // 传感器ID，默认0x64
    std::string full_version_string_;
    std::string hw_version_str_;
    std::string sw_version_str_;
    
    // 回调函数
    DataCallback data_callback_;
    ErrorCallback error_callback_;
    CANFrameCallback can_frame_callback_;
    
    
    // CAN监听相关
    std::atomic<bool> can_listener_running_;
    std::thread can_listener_thread_;
    
    std::mutex callback_mutex_;
    
    // CAN ID 计算方法
    uint32_t getCommandID() const { return sensor_id_; }      // 命令ID = sensor_id_
    uint32_t getDataID() const { return sensor_id_ + 1; }     // 数据ID = sensor_id_ + 1
    
    // 私有方法
    ErrorCode sendFrame(const CANFrame& frame);
    ErrorCode receiveFrame(CANFrame& frame, uint32_t timeout_ms);
    ErrorCode sendAndWaitResponse(const CANFrame& request, CANFrame& response, 
                                 uint32_t expected_id, uint32_t timeout_ms = 1000);
    void canListenerWorker();
    void stopCANListenerImpl();
    void closeCANImpl();
    void debugPrint(const std::string& message);
    void reportError(ErrorCode error, const std::string& message);
    CANFrame createFrame(uint32_t id, const uint8_t* data, uint8_t data_length);
    float bytesToFloat(const uint8_t* bytes, bool little_endian = true);
    void floatToBytes(float value, uint8_t* bytes, bool little_endian = true);
};

// ForceData 实现
std::string ForceData::toString() const {
    std::ostringstream oss;
    oss << std::fixed << std::setprecision(6) << std::right
        << " Fx:" << std::setw(10) << fx 
        << "  Fy:" << std::setw(10) << fy 
        << "  Fz:" << std::setw(10) << fz
        << "  Tx:" << std::setw(10) << tx 
        << "  Ty:" << std::setw(10) << ty 
        << "  Tz:" << std::setw(10) << tz;
    return oss.str();
}

// SixAxisSensorCANSDK 实现
SixAxisSensorCANSDK::SixAxisSensorCANSDK() 
    : pImpl(std::make_unique<Impl>()) {
}

SixAxisSensorCANSDK::~SixAxisSensorCANSDK() = default;

std::vector<std::string> SixAxisSensorCANSDK::scanCANInterfaces() {
    std::vector<std::string> interfaces;
    
#ifdef __linux__
    // 读取 /proc/net/dev 查找CAN接口
    std::ifstream proc_net("/proc/net/dev");
    if (proc_net.is_open()) {
        std::string line;
        // 跳过头部两行
        std::getline(proc_net, line);
        std::getline(proc_net, line);
        
        while (std::getline(proc_net, line)) {
            size_t colon = line.find(':');
            if (colon != std::string::npos) {
                std::string iface = line.substr(0, colon);
                // 去除空白字符
                iface.erase(0, iface.find_first_not_of(" \t"));
                iface.erase(iface.find_last_not_of(" \t") + 1);
                
                // 检查是否为CAN接口
                if (iface.find("can") == 0 || iface.find("vcan") == 0) {
                    interfaces.push_back(iface);
                }
            }
        }
        proc_net.close();
    }
#endif
    
    return interfaces;
}

ErrorCode SixAxisSensorCANSDK::initCAN(const std::string& interface_name, 
                                      uint32_t arbitration_bitrate,
                                      uint32_t data_bitrate,
                                      bool fd_mode,
                                      uint32_t sensor_id) {
    (void)arbitration_bitrate; // 标记未使用的参数
    (void)data_bitrate; // 标记未使用的参数  
    (void)fd_mode; // 标记未使用的参数
    
    pImpl->interface_name_ = interface_name;
    pImpl->sensor_id_ = sensor_id;  // 设置传感器ID
    
#ifdef __linux__
    // 创建CAN socket
    pImpl->can_socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (pImpl->can_socket_ < 0) {
        pImpl->reportError(ErrorCode::CAN_INIT_FAILED, "Failed to create CAN socket");
        return ErrorCode::CAN_INIT_FAILED;
    }
    
    // 启用CAN-FD支持（类似candump）
    int enable_canfd = 1;
    setsockopt(pImpl->can_socket_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, sizeof(enable_canfd));
    
    // 保持默认过滤器行为（类似candump默认行为）
    
    // 简化的CAN接口绑定 - 基于网上的标准例子
    struct sockaddr_can addr;
    struct ifreq ifr;
    
    // 指定CAN接口
    strcpy(ifr.ifr_name, interface_name.c_str());
    if (ioctl(pImpl->can_socket_, SIOCGIFINDEX, &ifr) < 0) {
        close(pImpl->can_socket_);
        pImpl->can_socket_ = -1;
        pImpl->reportError(ErrorCode::CAN_INIT_FAILED, "Failed to get interface index for " + interface_name);
        return ErrorCode::CAN_INIT_FAILED;
    }
    
    // 绑定socket到CAN接口
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;
    
    if (bind(pImpl->can_socket_, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        close(pImpl->can_socket_);
        pImpl->can_socket_ = -1;
        pImpl->reportError(ErrorCode::CAN_INIT_FAILED, "Failed to bind CAN socket to " + interface_name);
        return ErrorCode::CAN_INIT_FAILED;
    }
    
    pImpl->status_ = SensorStatus::CONNECTED;
    pImpl->debugPrint("CAN interface " + interface_name + " initialized successfully");
    return ErrorCode::SUCCESS;
#else
    pImpl->reportError(ErrorCode::CAN_INIT_FAILED, "CAN not supported on this platform");
    return ErrorCode::CAN_INIT_FAILED;
#endif
}

void SixAxisSensorCANSDK::closeCAN() {
    pImpl->stopCANListenerImpl();
    pImpl->closeCANImpl();
}

SensorStatus SixAxisSensorCANSDK::getStatus() const {
    return pImpl->status_;
}

uint32_t SixAxisSensorCANSDK::getSensorID() const {
    return pImpl->sensor_id_;
}

uint32_t SixAxisSensorCANSDK::getDataID() const {
    return pImpl->getDataID();
}

ErrorCode SixAxisSensorCANSDK::getVersion(uint16_t& hardware_version, uint16_t& software_version) {
    // 获取硬件信息(设备名称): [0x03, 0x00]
    uint8_t cmd_device_name[] = {0x03, 0x00};
    CANFrame request1 = pImpl->createFrame(pImpl->getCommandID(), cmd_device_name, 2);
    CANFrame response1;
    
    ErrorCode result = pImpl->sendAndWaitResponse(request1, response1, pImpl->getCommandID());
    if (result == ErrorCode::SUCCESS && response1.data_length >= 5 && 
        response1.data[0] == 0x03 && response1.data[1] == 0x00 && response1.data[2] == 0x00) {
        // 第4字节是有效数据长度
        uint8_t data_length = response1.data[3];
        if (data_length > 0 && response1.data_length >= (4 + data_length)) {
            // 按字符串直接解析设备名称
            std::string device_name(reinterpret_cast<const char*>(response1.data + 4), data_length);
            pImpl->full_version_string_ = device_name;
        }
    }
    
    // 获取软件版本: [0x03, 0x02] 
    uint8_t cmd_sw[] = {0x03, 0x02};
    CANFrame request_sw = pImpl->createFrame(pImpl->getCommandID(), cmd_sw, 2);
    CANFrame response_sw;
    
    result = pImpl->sendAndWaitResponse(request_sw, response_sw, pImpl->getCommandID());
    if (result == ErrorCode::SUCCESS && response_sw.data_length >= 5 && 
        response_sw.data[0] == 0x03 && response_sw.data[1] == 0x02 && response_sw.data[2] == 0x00) {
        // 第4字节是有效数据长度
        uint8_t data_length = response_sw.data[3];
        if (data_length >= 4 && response_sw.data_length >= (4 + data_length)) {
            // 按十进制解析 v2.0.2.7
            uint8_t major = response_sw.data[4];
            uint8_t minor = response_sw.data[5]; 
            uint8_t patch = response_sw.data[6];
            uint8_t build = response_sw.data[7];
            
            // 存储为 major*1000 + minor*100 + patch*10 + build = 2027
            software_version = major * 1000 + minor * 100 + patch * 10 + build;
            
            // 保存详细版本信息
            pImpl->sw_version_str_ = std::to_string(major) + "." + 
                                    std::to_string(minor) + "." +
                                    std::to_string(patch) + "." +
                                    std::to_string(build);
        }
    }
    
    // 硬件版本设为0，因为已删除03 01命令
    hardware_version = 0;
    pImpl->hw_version_str_ = "N/A";
    
    return ErrorCode::SUCCESS;
}

ErrorCode SixAxisSensorCANSDK::getFullVersionString(std::string& version_string) {
    version_string = pImpl->full_version_string_;
    if (version_string.empty()) {
        return ErrorCode::INVALID_DATA;
    }
    return ErrorCode::SUCCESS;
}

ErrorCode SixAxisSensorCANSDK::getFirmwareBuildTime(std::string& build_time) {
    // 读取编译时间: [0x03, 0x03]
    uint8_t cmd_data[] = {0x03, 0x03};
    CANFrame request = pImpl->createFrame(pImpl->getCommandID(), cmd_data, 2);
    CANFrame response;
    
    ErrorCode result = pImpl->sendAndWaitResponse(request, response, pImpl->getCommandID());
    if (result != ErrorCode::SUCCESS) {
        return result;
    }
    
    // 响应格式: [cmd, func, status, data_length, year, month, day, hour, minute, second, ...]
    if (response.data_length >= 5 && response.data[0] == 0x03 && 
        response.data[1] == 0x03 && response.data[2] == 0x00) {
        // 第4字节是有效数据长度
        uint8_t data_length = response.data[3];
        if (data_length >= 6 && response.data_length >= (4 + data_length)) {
            // 按十进制解析成25年9月12日14时53分17秒
            uint8_t year = response.data[4];   // 25
            uint8_t month = response.data[5];  // 9  
            uint8_t day = response.data[6];    // 12
            uint8_t hour = response.data[7];   // 14
            uint8_t minute = response.data[8]; // 53
            uint8_t second = response.data[9]; // 17
            
            // 格式化为: 2025年9月12日14时53分17秒
            std::ostringstream oss;
            oss << "20" << std::setfill('0') << std::setw(2) << (int)year 
                << "年" << (int)month << "月" << (int)day << "日"
                << std::setw(2) << (int)hour << "时"
                << std::setw(2) << (int)minute << "分"
                << std::setw(2) << (int)second << "秒";
            build_time = oss.str();
            return ErrorCode::SUCCESS;
        }
    }
    
    return ErrorCode::INVALID_DATA;
}

ErrorCode SixAxisSensorCANSDK::getSerialNumber(std::string& serial_number) {
    // 序列号读取命令: [0x03, 0x05] (CommandType=VERSION_INFO, FuncCode=0x05为序列号)
    uint8_t cmd_data[] = {0x03, 0x05};
    CANFrame request = pImpl->createFrame(pImpl->getCommandID(), cmd_data, 2);
    CANFrame response;
    
    ErrorCode result = pImpl->sendAndWaitResponse(request, response, 
                                                 pImpl->getCommandID());
    if (result != ErrorCode::SUCCESS) {
        return result;
    }
    
    // 响应格式: [cmd, func, status, ...serial_data]
    if (response.data_length >= 4 && response.data[2] == 0x00) { // status == SUCCESS
        if (response.data_length > 3) {
            serial_number = std::string(reinterpret_cast<const char*>(response.data + 3), 
                                       response.data_length - 3);
            return ErrorCode::SUCCESS;
        }
    }
    
    return ErrorCode::INVALID_DATA;
}

ErrorCode SixAxisSensorCANSDK::getDetailedVersionStrings(std::string& hw_version, std::string& sw_version) {
    // 硬件版本已删除03 01命令，直接返回设备名称
    hw_version = pImpl->full_version_string_.empty() ? "N/A" : pImpl->full_version_string_;
    
    // 获取软件版本: [0x03, 0x02]
    uint8_t cmd_data[] = {0x03, 0x02};
    CANFrame request = pImpl->createFrame(pImpl->getCommandID(), cmd_data, 2);
    CANFrame response;
    
    ErrorCode result = pImpl->sendAndWaitResponse(request, response, 
                                                 pImpl->getCommandID());
    if (result != ErrorCode::SUCCESS) {
        return result;
    }
    
    // 响应格式: [cmd, func, status, data_length, major, minor, patch, build, ...]
    if (response.data_length >= 5 && response.data[0] == 0x03 && 
        response.data[1] == 0x02 && response.data[2] == 0x00) {
        // 第4字节是有效数据长度
        uint8_t data_length = response.data[3];
        if (data_length >= 4 && response.data_length >= (4 + data_length)) {
            // 按十进制解析 v2.0.2.7
            uint8_t major = response.data[4];
            uint8_t minor = response.data[5]; 
            uint8_t patch = response.data[6];
            uint8_t build = response.data[7];
            
            sw_version = std::to_string(major) + "." + std::to_string(minor) + "." + 
                        std::to_string(patch) + "." + std::to_string(build);
        } else {
            return ErrorCode::INVALID_DATA;
        }
    } else {
        return ErrorCode::INVALID_DATA;
    }
    
    return ErrorCode::SUCCESS;
}


ErrorCode SixAxisSensorCANSDK::parseFTData(const CANFrame& frame, ForceData& data) {
    if (frame.data_length < 24) { // 6个float * 4字节 = 24字节
        return ErrorCode::INVALID_DATA;
    }
    
    data.fx = pImpl->bytesToFloat(&frame.data[0]);
    data.fy = pImpl->bytesToFloat(&frame.data[4]);
    data.fz = pImpl->bytesToFloat(&frame.data[8]);
    data.tx = pImpl->bytesToFloat(&frame.data[12]);
    data.ty = pImpl->bytesToFloat(&frame.data[16]);
    data.tz = pImpl->bytesToFloat(&frame.data[20]);
    
    data.timestamp = std::chrono::system_clock::now();
    
    // 验证数据有效性
    if (std::isnan(data.fx) || std::isnan(data.fy) || std::isnan(data.fz) ||
        std::isnan(data.tx) || std::isnan(data.ty) || std::isnan(data.tz)) {
        return ErrorCode::INVALID_DATA;
    }
    
    return ErrorCode::SUCCESS;
}

ErrorCode SixAxisSensorCANSDK::setTareFunction(bool enable) {
    // 去皮命令: [0x01, 0x09, enable] 后面带参数1开启或0关闭
    uint8_t cmd_data[] = {0x01, 0x09, enable ? (uint8_t)0x01 : (uint8_t)0x00};
    CANFrame request = pImpl->createFrame(pImpl->getCommandID(), cmd_data, 3);
    CANFrame response;
    
    ErrorCode result = pImpl->sendAndWaitResponse(request, response, 
                                                 pImpl->getCommandID());
    if (result != ErrorCode::SUCCESS) {
        return result;
    }
    
    // 检查响应状态
    if (response.data_length >= 3 && response.data[0] == 0x01 && response.data[1] == 0x09) {
        if (response.data[2] == 0x00) { // status == SUCCESS
            return ErrorCode::SUCCESS;
        }
    }
    return ErrorCode::COMMUNICATION_FAILED;
}

ErrorCode SixAxisSensorCANSDK::changeSensorID(uint32_t new_sensor_id) {
    // CAN ID修改命令: [0x01, 0x04, id_bytes...] 其中id_bytes是32位小端序
    uint8_t cmd_data[6];
    cmd_data[0] = 0x01;  // WRITE_PARAM
    cmd_data[1] = 0x04;  // CAN ID参数
    
    // 32位ID转换为小端序字节
    cmd_data[2] = new_sensor_id & 0xFF;
    cmd_data[3] = (new_sensor_id >> 8) & 0xFF;
    cmd_data[4] = (new_sensor_id >> 16) & 0xFF;
    cmd_data[5] = (new_sensor_id >> 24) & 0xFF;
    
    CANFrame request = pImpl->createFrame(pImpl->getCommandID(), cmd_data, 6);
    CANFrame response;
    
    ErrorCode result = pImpl->sendAndWaitResponse(request, response, 
                                                 pImpl->getCommandID());
    if (result != ErrorCode::SUCCESS) {
        return result;
    }
    
    // 检查响应状态
    if (response.data_length >= 3 && response.data[0] == 0x01 && response.data[1] == 0x04) {
        if (response.data[2] == 0x00) { // status == SUCCESS
            // 更新本地传感器ID
            pImpl->sensor_id_ = new_sensor_id;
            return ErrorCode::SUCCESS;
        }
    }
    return ErrorCode::COMMUNICATION_FAILED;
}

ErrorCode SixAxisSensorCANSDK::saveParametersToFlash() {
    // 保存参数命令: [0x02, 0x00, 0xAA, 0xAA]
    uint8_t cmd_data[] = {0x02, 0x00, 0xAA, 0xAA};
    CANFrame request = pImpl->createFrame(pImpl->getCommandID(), cmd_data, 4);
    CANFrame response;
    
    ErrorCode result = pImpl->sendAndWaitResponse(request, response, 
                                                 pImpl->getCommandID(), 3000); // 3秒超时，flash写入需要时间
    if (result != ErrorCode::SUCCESS) {
        return result;
    }
    
    // 检查响应状态
    if (response.data_length >= 3 && response.data[0] == 0x02 && response.data[1] == 0x00) {
        if (response.data[2] == 0x00) { // status == SUCCESS
            return ErrorCode::SUCCESS;
        }
    }
    return ErrorCode::COMMUNICATION_FAILED;
}





void SixAxisSensorCANSDK::setCANFrameCallback(CANFrameCallback callback) {
    std::lock_guard<std::mutex> lock(pImpl->callback_mutex_);
    pImpl->can_frame_callback_ = callback;
}

ErrorCode SixAxisSensorCANSDK::startCANListener() {
    if (pImpl->can_listener_running_.load()) {
        return ErrorCode::PROTOCOL_ERROR; // 已在运行
    }
    
    pImpl->can_listener_running_ = true;
    pImpl->can_listener_thread_ = std::thread(&SixAxisSensorCANSDK::Impl::canListenerWorker, 
                                             pImpl.get());
    
    pImpl->debugPrint("CAN listener started");
    return ErrorCode::SUCCESS;
}

void SixAxisSensorCANSDK::stopCANListener() {
    pImpl->stopCANListenerImpl();
}

void SixAxisSensorCANSDK::setErrorCallback(ErrorCallback callback) {
    std::lock_guard<std::mutex> lock(pImpl->callback_mutex_);
    pImpl->error_callback_ = callback;
}

void SixAxisSensorCANSDK::setDebugMode(bool enable) {
    pImpl->debug_mode_ = enable;
}

std::string SixAxisSensorCANSDK::getSDKVersion() {
    return "1.0.0";
}

// Impl 私有方法实现
ErrorCode SixAxisSensorCANSDK::Impl::sendFrame(const CANFrame& frame) {
#ifdef __linux__
    if (can_socket_ < 0) {
        return ErrorCode::CAN_INIT_FAILED;
    }
    
    struct can_frame can_frame;
    can_frame.can_id = frame.id;
    can_frame.can_dlc = frame.data_length;
    memcpy(can_frame.data, frame.data, frame.data_length);
    
    ssize_t nbytes = write(can_socket_, &can_frame, sizeof(struct can_frame));
    if (nbytes != sizeof(struct can_frame)) {
        reportError(ErrorCode::CAN_SEND_FAILED, "Failed to send CAN frame");
        return ErrorCode::CAN_SEND_FAILED;
    }
    
    std::ostringstream oss;
    oss << "Sent CAN frame ID: 0x" << std::hex << frame.id;
    debugPrint(oss.str());
    return ErrorCode::SUCCESS;
#else
    return ErrorCode::CAN_SEND_FAILED;
#endif
}

ErrorCode SixAxisSensorCANSDK::Impl::receiveFrame(CANFrame& frame, uint32_t timeout_ms) {
#ifdef __linux__
    (void)timeout_ms; // 暂时不使用超时，直接读取
    
    if (can_socket_ < 0) {
        return ErrorCode::CAN_INIT_FAILED;
    }
    
    // 完全模仿candump的方式 - 使用recvmsg
    union {
        struct can_frame cc;
        struct canfd_frame fd;
    } cu;
    
    struct iovec iov;
    struct msghdr msg;
    struct sockaddr_can addr;
    
    iov.iov_base = &cu;
    iov.iov_len = sizeof(cu);
    msg.msg_name = &addr;
    msg.msg_namelen = sizeof(addr);
    msg.msg_iov = &iov;
    msg.msg_iovlen = 1;
    msg.msg_control = nullptr;
    msg.msg_controllen = 0;
    msg.msg_flags = 0;
    
    debugPrint("About to call recvmsg...");
    ssize_t nbytes = recvmsg(can_socket_, &msg, 0);
    debugPrint("recvmsg returned: " + std::to_string(nbytes));
    
    if (nbytes <= 0) {
        debugPrint("recvmsg failed: " + std::string(strerror(errno)));
        return ErrorCode::CAN_RECEIVE_TIMEOUT;
    }
    
    // 根据candump的逻辑判断帧类型
    if (nbytes == CAN_MTU) {
        // 标准CAN帧
        frame.id = cu.cc.can_id;
        frame.frame_type = CANFrameType::STANDARD;
        frame.data_length = cu.cc.can_dlc;
        memcpy(frame.data, cu.cc.data, cu.cc.can_dlc);
    } else if (nbytes == CANFD_MTU) {
        // CAN-FD帧
        frame.id = cu.fd.can_id;
        frame.frame_type = CANFrameType::STANDARD;
        frame.data_length = cu.fd.len;
        memcpy(frame.data, cu.fd.data, cu.fd.len);
    } else {
        reportError(ErrorCode::INVALID_DATA, "Invalid CAN frame size: " + std::to_string(nbytes));
        return ErrorCode::INVALID_DATA;
    }
    
    frame.timestamp = std::chrono::system_clock::now();
    debugPrint("Received CAN frame ID: 0x" + std::to_string(frame.id) + 
               ", len=" + std::to_string(frame.data_length) + ", nbytes=" + std::to_string(nbytes));
    return ErrorCode::SUCCESS;
#else
    return ErrorCode::CAN_RECEIVE_TIMEOUT;
#endif
}

ErrorCode SixAxisSensorCANSDK::Impl::sendAndWaitResponse(const CANFrame& request, 
                                                       CANFrame& response, 
                                                       uint32_t expected_id, 
                                                       uint32_t timeout_ms) {
    ErrorCode result = sendFrame(request);
    if (result != ErrorCode::SUCCESS) {
        return result;
    }
    
    auto start_time = std::chrono::steady_clock::now();
    
    while (true) {
        result = receiveFrame(response, 100); // 100ms timeout per attempt
        if (result == ErrorCode::SUCCESS) {
            if (response.id == expected_id) {
                return ErrorCode::SUCCESS;
            }
            // 收到了其他消息，继续等待
        } else if (result != ErrorCode::CAN_RECEIVE_TIMEOUT) {
            return result;
        }
        
        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
            std::chrono::steady_clock::now() - start_time).count();
        
        if (elapsed >= timeout_ms) {
            return ErrorCode::CAN_RECEIVE_TIMEOUT;
        }
    }
}


void SixAxisSensorCANSDK::Impl::canListenerWorker() {
    debugPrint("CAN listener worker started");
    int loop_count = 0;
    while (can_listener_running_.load()) {
        CANFrame frame;
        ErrorCode result = receiveFrame(frame, 100); // 100ms timeout
        
        loop_count++;
        if (loop_count % 10 == 1) { // 每10次循环打印一次状态
            debugPrint("CAN listener loop " + std::to_string(loop_count) + ", result: " + std::to_string(static_cast<int>(result)));
        }
        
        if (result == ErrorCode::SUCCESS) {
            debugPrint("CAN frame received, calling callback");
            std::lock_guard<std::mutex> lock(callback_mutex_);
            if (can_frame_callback_) {
                can_frame_callback_(frame);
            }
        } else if (result != ErrorCode::CAN_RECEIVE_TIMEOUT) {
            // 非超时错误，报告错误
            reportError(result, "CAN listener error");
            break;
        }
    }
    debugPrint("CAN listener worker stopped");
}

void SixAxisSensorCANSDK::Impl::debugPrint(const std::string& message) {
    if (debug_mode_) {
        std::cout << "[CAN SDK] " << message << std::endl;
    }
}

void SixAxisSensorCANSDK::Impl::reportError(ErrorCode error, const std::string& message) {
    std::lock_guard<std::mutex> lock(callback_mutex_);
    if (error_callback_) {
        error_callback_(error, message);
    } else if (debug_mode_) {
        std::cerr << "[CAN SDK ERROR] " << message << " (Code: " << static_cast<int>(error) << ")" << std::endl;
    }
}

CANFrame SixAxisSensorCANSDK::Impl::createFrame(uint32_t id, const uint8_t* data, uint8_t data_length) {
    CANFrame frame;
    frame.id = id;
    frame.frame_type = CANFrameType::STANDARD;
    frame.data_length = data_length;
    
    if (data && data_length > 0) {
        memcpy(frame.data, data, data_length);
    }
    
    frame.timestamp = std::chrono::system_clock::now();
    return frame;
}

float SixAxisSensorCANSDK::Impl::bytesToFloat(const uint8_t* bytes, bool little_endian) {
    union {
        float f;
        uint8_t b[4];
    } u;
    
    if (little_endian) {
        u.b[0] = bytes[0];
        u.b[1] = bytes[1];
        u.b[2] = bytes[2];
        u.b[3] = bytes[3];
    } else {
        u.b[0] = bytes[3];
        u.b[1] = bytes[2];
        u.b[2] = bytes[1];
        u.b[3] = bytes[0];
    }
    
    return u.f;
}

void SixAxisSensorCANSDK::Impl::floatToBytes(float value, uint8_t* bytes, bool little_endian) {
    union {
        float f;
        uint8_t b[4];
    } u;
    
    u.f = value;
    
    if (little_endian) {
        bytes[0] = u.b[0];
        bytes[1] = u.b[1];
        bytes[2] = u.b[2];
        bytes[3] = u.b[3];
    } else {
        bytes[0] = u.b[3];
        bytes[1] = u.b[2];
        bytes[2] = u.b[1];
        bytes[3] = u.b[0];
    }
}


void SixAxisSensorCANSDK::Impl::stopCANListenerImpl() {
    if (can_listener_running_.load()) {
        can_listener_running_ = false;
        if (can_listener_thread_.joinable()) {
            can_listener_thread_.join();
        }
        debugPrint("CAN listener stopped");
    }
}

void SixAxisSensorCANSDK::Impl::closeCANImpl() {
#ifdef __linux__
    if (can_socket_ >= 0) {
        close(can_socket_);
        can_socket_ = -1;
    }
#endif
    
    status_ = SensorStatus::DISCONNECTED;
    debugPrint("CAN connection closed");
}

} // namespace SixAxisSensorCAN